{
 "cells": [
  {
   "cell_type": "markdown",
   "id": "exempt-legislation",
   "metadata": {},
   "source": [
    "# Gain Scheduling\n",
    "\n",
    "##### Richard M. Murray, 19 Nov 2021 (updated 7 Jul 2024)\n",
    "\n",
    "This notebook contains an example of using gain scheduling for feedback control of a nonlinear system.  A gain scheduled controller has feedback gains that depend on a set of measured parameters in the system. For exampe:\n",
    "\n",
    "$$\n",
    "  u = u_\\text{d} − K(x_\\text{d}, u_\\text{d}) (x − x_\\text{d}),\n",
    "$$\n",
    "\n",
    "where $K(x_\\text{d}, u_\\text{d})$ depends on the desired system state and input.\n",
    "\n",
    "In this notebook, we work through the gain scheduled controller in Example 2.1 of OBC."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "corresponding-convenience",
   "metadata": {},
   "outputs": [],
   "source": [
    "# Import the packages needed for the examples included in this notebook\n",
    "import numpy as np\n",
    "import matplotlib.pyplot as plt\n",
    "from cmath import sqrt\n",
    "import control as ct"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "corporate-sense",
   "metadata": {},
   "source": [
    "## Vehicle Steering Dynamics\n",
    "\n",
    "The vehicle dynamics are given by a simple bicycle model:\n",
    "\n",
    "<table>\n",
    "<tr>\n",
    "    <td width=\"50%\"><img src=\"https://fbswiki.org/wiki/images/5/52/Kincar.png\" width=480></td>\n",
    "    <td width=\"50%\">\n",
    "$$\n",
    "\\begin{aligned}\n",
    "  \\dot x &= \\cos\\theta\\, v \\\\\n",
    "  \\dot y &= \\sin\\theta\\, v \\\\\n",
    "  \\dot\\theta &= \\frac{v}{l} \\tan \\delta\n",
    "\\end{aligned}\n",
    "$$\n",
    "    </td>\n",
    "</tr>\n",
    "</table>\n",
    "\n",
    "We take the state of the system as $(x, y, \\theta)$ where $(x, y)$ is the position of the vehicle in the plane and $\\theta$ is the angle of the vehicle with respect to horizontal.  The vehicle input is given by $(v, \\delta)$ where $v$ is the forward velocity of the vehicle and $\\delta$ is the angle of the steering wheel.  The model includes saturation of the vehicle steering angle."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "naval-pizza",
   "metadata": {},
   "outputs": [],
   "source": [
    "# Bicycle model dynamics\n",
    "#\n",
    "# System state: x, y, theta\n",
    "# System input: v, delta\n",
    "# System output: x, y\n",
    "# System parameters: wheelbase, maxsteer\n",
    "#\n",
    "def bicycle_update(t, x, u, params):\n",
    "    # Get the parameters for the model\n",
    "    l = params.get('wheelbase', 3.)         # vehicle wheelbase\n",
    "    deltamax = params.get('maxsteer', 0.5)  # max steering angle (rad)\n",
    "\n",
    "    # Saturate the steering input\n",
    "    delta = np.clip(u[1], -deltamax, deltamax)\n",
    "\n",
    "    # Return the derivative of the state\n",
    "    return np.array([\n",
    "        np.cos(x[2]) * u[0],            # xdot = cos(theta) v\n",
    "        np.sin(x[2]) * u[0],            # ydot = sin(theta) v\n",
    "        (u[0] / l) * np.tan(delta)      # thdot = v/l tan(delta)\n",
    "    ])\n",
    "\n",
    "def bicycle_output(t, x, u, params):\n",
    "    return x                            # return x, y, theta (full state)\n",
    "\n",
    "# Define the vehicle steering dynamics as an input/output system\n",
    "bicycle = ct.nlsys(\n",
    "    bicycle_update, bicycle_output, states=3, name='bicycle',\n",
    "    inputs=('v', 'delta'),\n",
    "    outputs=('x', 'y', 'theta'))"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "3cc26675",
   "metadata": {},
   "source": [
    "## Gain scheduled controller\n",
    "\n",
    "For this system we use a simple schedule on the forward vehicle velocity and\n",
    "place the poles of the system at fixed values.  The controller takes the\n",
    "current and desired vehicle position and orientation plus the velocity\n",
    "velocity as inputs, and returns the velocity and steering commands.\n",
    "\n",
    "Linearizing the system about the desired trajectory, we obtain\n",
    "\n",
    "$$\n",
    "  \\begin{aligned}\n",
    "    A(x_\\text{d}) &= \\left. \\frac{\\partial f}{\\partial x} \\right|_{(x_\\text{d}, u_\\text{d})}\n",
    "      = \\left.\n",
    "        \\begin{bmatrix}\n",
    "          0 & 0 & -\\sin\\theta_\\text{d}\\, v_\\text{d} \\\\ 0 & 0 & \\cos\\theta_\\text{d}\\, v_\\text{d} \\\\ 0 & 0 & 0\n",
    "        \\end{bmatrix}\n",
    "        \\right|_{(x_\\text{d}, u_\\text{d})}\n",
    "      = \\begin{bmatrix}\n",
    "          0 & 0 & 0 \\\\ 0 & 0 & v_\\text{d} \\\\ 0 & 0 & 0\n",
    "         \\end{bmatrix}, \\\\\n",
    "    B(x_\\text{d}) &= \\left. \\frac{\\partial f}{\\partial u} \\right|_{(x_\\text{d}, u_\\text{d})}\n",
    "     = \\begin{bmatrix}\n",
    "       1 & 0 \\\\ 0 & 0 \\\\ 0 & v_\\text{d}/l\n",
    "       \\end{bmatrix}.\n",
    "  \\end{aligned}\n",
    "$$\n",
    "\n",
    "We form the error dynamics by setting $e = x - x_\\text{d}$ and $w = u -\n",
    "u_\\text{d}$:\n",
    "$$\n",
    "  \\dot e_x = w_1, \\qquad \\dot e_y = e_\\theta, \\qquad \\dot e_\\theta =\n",
    "  \\frac{v_\\text{d}}{l} w_2.\n",
    "$$\n",
    "We see that the first state is decoupled from the second two states\n",
    "and hence we can design a controller by treating these two subsystems\n",
    "separately.  \n",
    "\n",
    "Suppose that we wish to place the closed loop eigenvalues\n",
    "of the longitudinal dynamics ($e_x$) at $-\\lambda_1$ and place the\n",
    "closed loop eigenvalues of the lateral dynamics ($e_y$, $e_\\theta$) at\n",
    "the roots of the polynomial equation $s^2 + a_1 s + a_2 = 0$.\n",
    "\n",
    "This can accomplished by setting\n",
    "\n",
    "$$\n",
    "  \\begin{aligned}\n",
    "    w_1 &= -\\lambda_1 e_x \\\\\n",
    "    w_2 &= -\\frac{l}{v_\\text{r}}(\\frac{a_2}{v_\\text{r}} e_y + a_1 e_\\theta).\n",
    "  \\end{aligned}\n",
    "$$\n",
    "\n",
    "Note that the gains depend on the velocity $v_\\text{r}$ (or equivalently on\n",
    "the nominal input $u_\\text{d}$), giving us a gain scheduled controller."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "another-milwaukee",
   "metadata": {},
   "outputs": [],
   "source": [
    "# System state: none\n",
    "# System input: x, y, theta, xd, yd, thetad, vd, delta\n",
    "# System output: v, delta\n",
    "# System parameters: longpole, latomega_c, latzeta_c\n",
    "def gainsched_output(t, x, u, params):\n",
    "    # Get the controller parameters\n",
    "    longpole = params.get('longpole', -2.)\n",
    "    latomega_c = params.get('latomega_c', 2)\n",
    "    latzeta_c = params.get('latzeta_c', 0.5)\n",
    "    l = params.get('wheelbase', 3)\n",
    "    vref = params.get('vref', None)\n",
    "    \n",
    "    # Extract the system inputs and compute the errors\n",
    "    x, y, theta, xd, yd, thetad, vd, deltad = u\n",
    "    ex, ey, etheta = x - xd, y - yd, theta - thetad\n",
    "\n",
    "    # Determine the controller gains\n",
    "    lambda1 = -longpole\n",
    "    a1 = 2 * latzeta_c * latomega_c\n",
    "    a2 = latomega_c**2\n",
    "    \n",
    "    # Determine the speed to use for computing the gains\n",
    "    if vref is None:\n",
    "        vref = vd\n",
    "\n",
    "    # Compute and return the control law\n",
    "    v = -lambda1 * ex           # leave off feedforward to generate transient\n",
    "    if vd != 0:\n",
    "        delta = deltad - ((a2 * l) / vref**2) * ey - ((a1 * l) / vref) * etheta\n",
    "    else:\n",
    "        # We aren't moving, so don't turn the steering wheel\n",
    "        delta = deltad\n",
    "    \n",
    "    return  np.array([v, delta])\n",
    "\n",
    "# Define the controller as an input/output system\n",
    "gainsched = ct.nlsys(\n",
    "    None, gainsched_output, name='controller',          # static system\n",
    "    inputs=('x', 'y', 'theta', 'xd', 'yd', 'thetad',    # system inputs\n",
    "            'vd', 'deltad'),\n",
    "    outputs=('v', 'delta')                              # system outputs\n",
    ")"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "6c6c4b9b",
   "metadata": {},
   "source": [
    "## Reference trajectory subsystem\n",
    "\n",
    "The reference trajectory block generates a simple trajectory for the system\n",
    "given the desired speed (vref) and lateral position (yref).  The trajectory\n",
    "consists of a straight line of the form (vref * t, yref, 0) with nominal\n",
    "input (vref, 0)."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "significant-november",
   "metadata": {},
   "outputs": [],
   "source": [
    "# System state: none\n",
    "# System input: vref, yref\n",
    "# System output: xd, yd, thetad, vd, deltad\n",
    "# System parameters: none\n",
    "#\n",
    "def trajgen_output(t, x, u, params):\n",
    "    vref, yref = u\n",
    "    return np.array([vref * t, yref, 0, vref, 0])\n",
    "\n",
    "# Define the trajectory generator as an input/output system\n",
    "trajgen = ct.nlsys(\n",
    "    None, trajgen_output, name='trajgen',\n",
    "    inputs=('vref', 'yref'),\n",
    "    outputs=('xd', 'yd', 'thetad', 'vd', 'deltad'))\n"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "4ca5ab53",
   "metadata": {},
   "source": [
    "## System construction\n",
    "\n",
    "The input to the full closed loop system is the desired lateral position and\n",
    "the desired forward velocity.  The output for the system is taken as the\n",
    "full vehicle state plus the velocity of the vehicle.\n",
    "\n",
    "We construct the system using the InterconnectedSystem constructor and using\n",
    "signal labels to keep track of everything.  "
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "editorial-satisfaction",
   "metadata": {},
   "outputs": [],
   "source": [
    "steering_gainsched = ct.interconnect(\n",
    "    # List of subsystems\n",
    "    (trajgen, gainsched, bicycle), name='steering',\n",
    "\n",
    "    # System inputs\n",
    "    inplist=['trajgen.vref', 'trajgen.yref'],\n",
    "    inputs=['yref', 'vref'],\n",
    "\n",
    "    #  System outputs\n",
    "    outlist=['bicycle.x', 'bicycle.y', 'bicycle.theta', 'controller.v',\n",
    "             'controller.delta'],\n",
    "    outputs=['x', 'y', 'theta', 'v', 'delta']\n",
    ")"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "61fe3404",
   "metadata": {},
   "source": [
    "Note the use of signals of the form `sys.sig` to get the signals from a specific subsystem."
   ]
  },
  {
   "cell_type": "markdown",
   "id": "47f5d528",
   "metadata": {},
   "source": [
    "## System simulation"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "smoking-trail",
   "metadata": {},
   "outputs": [],
   "source": [
    "# Set up the simulation conditions\n",
    "yref = 1\n",
    "T = np.linspace(0, 5, 100)\n",
    "\n",
    "# Plot the reference trajectory for the y position\n",
    "plt.plot([0, 5], [yref, yref], 'k-', linewidth=0.6)\n",
    "\n",
    "# Find the signals we want to plot\n",
    "y_index = steering_gainsched.find_output('y')\n",
    "v_index = steering_gainsched.find_output('v')\n",
    "\n",
    "# Do an iteration through different speeds\n",
    "for vref in [5, 10, 15]:\n",
    "    # Simulate the closed loop controller response\n",
    "    tout, yout = ct.input_output_response(\n",
    "        steering_gainsched, T, [vref * np.ones(len(T)), yref * np.ones(len(T))])\n",
    "\n",
    "    # Plot the reference speed\n",
    "    plt.plot([0, 5], [vref, vref], 'k-', linewidth=0.6)\n",
    "\n",
    "    # Plot the system output\n",
    "    y_line, = plt.plot(tout, yout[y_index, :], 'r-')  # lateral position\n",
    "    v_line, = plt.plot(tout, yout[v_index, :], 'b--')  # vehicle velocity\n",
    "\n",
    "# Add axis labels\n",
    "plt.xlabel(\"Time [s]\")\n",
    "plt.ylabel(r\"$\\dot{x}$ [m/s], $y$ [m]\")\n",
    "plt.legend((v_line, y_line), (r\"$\\dot{x}$\", \"$y$\"),\n",
    "           loc='center right', frameon=False);"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "8f31bc48",
   "metadata": {},
   "source": [
    "## Comparison to fixed controller"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "homeless-gibson",
   "metadata": {},
   "outputs": [],
   "source": [
    "# Rerun with no gain-scheduling\n",
    "\n",
    "# Plot the reference trajectory for the y position\n",
    "plt.plot([0, 5], [yref, yref], 'k-', linewidth=0.6)\n",
    "\n",
    "# Do an iteration through different speeds\n",
    "for vref in [5, 10, 15]:\n",
    "    # Simulate the closed loop controller response\n",
    "    tout, yout = ct.input_output_response(\n",
    "        steering_gainsched, T, [vref * np.ones(len(T)), yref * np.ones(len(T))], \n",
    "        params={'vref': 15})\n",
    "\n",
    "    # Plot the reference speed\n",
    "    plt.plot([0, 5], [vref, vref], 'k-', linewidth=0.6)\n",
    "\n",
    "    # Plot the system output\n",
    "    y_line, = plt.plot(tout, yout[y_index, :], 'r-')  # lateral position\n",
    "    v_line, = plt.plot(tout, yout[v_index, :], 'b--')  # vehicle velocity\n",
    "\n",
    "# Add axis labels\n",
    "plt.xlabel(\"Time [s]\")\n",
    "plt.ylabel(r\"$\\dot{x}$ [m/s], $y$ [m]\")\n",
    "plt.legend((v_line, y_line), (r\"$\\dot{x}$\", \"$y$\"),\n",
    "           loc='center right', frameon=False);"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "5811a6e4",
   "metadata": {},
   "source": [
    "## Things to try\n",
    "* Use different reference trajectories (eg, flatness-based trajectory)\n",
    "* Try scheduling on the current state rather than the desired state"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "6f571b2b",
   "metadata": {},
   "outputs": [],
   "source": []
  }
 ],
 "metadata": {
  "language_info": {
   "name": "python"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 5
}
